#ifndef __FOC_H
#define __FOC_H

#include "stm32f1xx_hal.h"
#include "tim.h"
#include "adc.h"

#define _constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))
#define _2_SQRT3 1.15470053838
#define _SQRT3 1.73205080757
#define _1_SQRT3 0.57735026919
#define _SQRT3_2 0.86602540378
#define _SQRT2 1.41421356237
#define _120_D2R 2.09439510239
#define _PI 3.14159265359
#define _PI_2 1.57079632679
#define _PI_3 1.0471975512
#define _2PI 6.28318530718
#define _3PI_2 4.71238898038
#define _PI_6 0.52359877559

#define SetPWM_A(duty) __HAL_TIM_SetCompare(&htim1,TIM_CHANNEL_1,duty*7200)
#define SetPWM_B(duty) __HAL_TIM_SetCompare(&htim1,TIM_CHANNEL_2,duty*7200)
#define SetPWM_C(duty) __HAL_TIM_SetCompare(&htim1,TIM_CHANNEL_3,duty*7200)
		
extern int PP,DIR,voltage_power_supply;

//extern float voltage_limit = 10;
//extern float shaft_angle = 0,open_loop_timestamp = 0;
extern float Ualpha,Ubeta,Ua,Ub,Uc,dc_a,dc_b,dc_c;

//float Sensor_Angle;			//无刷电机实际角度
//float Angle_target = 3.00;	//无刷电机目标角度
//float Sensor_Speed;			//无刷电机实际速度
//float Speed_target = 20.0;	//无刷电机目标速度

float _shaft_angle(void);
float _electricalAngle(void);
float _normalizeAngle(float angle);
void setPhaseVoltage(float Uq,float Ud, float angle_el);
void Park_Transf(float i_alpha,float i_beta,float iq,float id,float angle_el);
void Clarke_Transf(float *iphase,float i_alpha,float i_beta);
float setPhaseCurrent(float Iq,float Id, float angle_el,float *current_value,float *vofa);
float FOC_init(void);
float velocitycloseloop(float target_velocity,float Real_velocity);
float positioncloseloop(float target_position,float feedback_position);
void get_current_test(float *current_value);
float currentcloseloop(float target_current,float feedback_current);
	
#endif
